import { Agent } from "./Agent";
import { Obstacle, RVOMath, Vector2 } from "./Common";
import { KdTree } from "./kdtree";

/**
 * RVO 模拟器
 * 
 * @优化说明
 * - 批量处理代理操作
 * - 优化循环性能
 * - 添加性能监控
 */
export class Simulator {
    private agentId: number = 0;
    private agentIdLst: number[] = [];
    aid2agent: { [key: string]: Agent } = Object.create(null);

    obstacles: Obstacle[] = [];
    kdTree: KdTree = new KdTree();

    defaultAgent: Agent | null = null; // Agent
    time: number = 0.0;
    
    // 性能统计
    private stats = {
        buildTreeTime: 0,
        computeNeighborsTime: 0,
        computeVelocityTime: 0,
        updateTime: 0,
        totalFrames: 0
    };

    private static _inst: Simulator;
    static get instance(): Simulator {
        if (!Simulator._inst) {
            Simulator._inst = new Simulator();
        }
        return Simulator._inst;
    }

    getAgent(idx: number) {
        return this.aid2agent[this.agentIdLst[idx]];
    }

    getAgentByAid(aid: number) {
        return this.aid2agent[aid];
    }

    getGlobalTime() {
        return this.time;
    };

    getNumAgents() {
        return this.agentIdLst.length;
    };

    getAgentAidByIdx(idx: number) {
        return this.agentIdLst[idx]
    }

    /**
     * 设置代理的期望速度
     */
    setAgentPrefVelocity(aid: number, velocity: Vector2 | { x: number; y: number }) {
        this.aid2agent[aid].prefVelocity_.copy(velocity);
    }

    getAgentPosition(aid: number) {
        if (this.aid2agent[aid]) {//为什么移除了 还会进入这个aid的检测
            return this.aid2agent[aid].position_;
        }
        return null
    }

    getAgentPrefVelocity(aid: number) {
        return this.aid2agent[aid].prefVelocity_;
    }

    getAgentVelocity(aid: number) {
        return this.aid2agent[aid].velocity_;
    }

    getAgentRadius(aid: number) {
        return this.aid2agent[aid].radius_;
    }

    getAgentOrcaLines(aid: number) {
        return this.aid2agent[aid].orcaLines_;
    }

    /**
     * 添加动态避障管理对象
     * @param position 初始位置
     * @param radius  检测半径
     * @param maxSpeed  最大速度
     * @param velocity 初始线速度(向量)
     * @param mass 转向质量
     * @returns 代理 ID
     */
    addAgent(
        position: Vector2 | { x: number; y: number },
        radius: number | null = null,
        maxSpeed: number | null = null,
        velocity: Vector2 | null = null,
        mass: number | null = null
    ): number {
        if (!this.defaultAgent) {
            throw new Error("no default agent");
        }

        let agent = new Agent();

        agent.position_.copy(position);
        agent.maxNeighbors_ = this.defaultAgent.maxNeighbors_;
        agent.maxSpeed_ = maxSpeed || this.defaultAgent.maxSpeed_;
        agent.neighborDist = this.defaultAgent.neighborDist;
        agent.radius_ = radius || this.defaultAgent.radius_;
        agent.timeHorizon = this.defaultAgent.timeHorizon;
        agent.timeHorizonObst = this.defaultAgent.timeHorizonObst;
        agent.velocity_.copy(velocity || this.defaultAgent.velocity_);

        agent.id = this.agentId++;

        if (mass && mass >= 0) {
            agent.mass = mass
        }
        this.aid2agent[agent.id] = agent;
        this.agentIdLst.push(agent.id);

        return agent.id;
    }

    removeAgent(aid: number) {
        if (this.hasAgent(aid)) {

            let idx = this.agentIdLst.indexOf(aid);
            if (idx >= 0) {
                // this.agentIdLst.splice(idx, 1) //用高效伪移除
                this.agentIdLst[idx] = this.agentIdLst[this.agentIdLst.length - 1];
                this.agentIdLst.length--;
            }
            delete this.aid2agent[aid];
        }
    }

    hasAgent(aid: number) {
        return !!this.aid2agent[aid];
    }

    setAgentMass(agentNo: number, mass: number) {
        this.aid2agent[agentNo].mass = mass;
    }

    getAgentMass(agentNo: number) {
        return this.aid2agent[agentNo].mass;
    }

    setAgentRadius(agentNo: number, radius: number) {
        this.aid2agent[agentNo].radius_ = radius;
    }

    /**
     * 
     * @param neighborDist 在寻找周围邻居的搜索距离，这个值设置过大，会让小球在很远距离时做出避障行为
     * @param maxNeighbors 寻找周围邻居的最大数目，这个值设置越大，最终计算的速度越精确，但会增大计算量
     * @param timeHorizon 代表计算动态的物体时的时间窗口
     * @param timeHorizonObst 代表计算静态的物体时的时间窗口，比如在RTS游戏中，小兵向城墙移动时，没必要做出避障，这个值需要 设置得很小
     * @param radius 代表计算ORCA时的小球的半径，这个值不一定与小球实际显示的半径一样，偏小有利于小球移动顺畅
     * @param maxSpeed 小球最大速度值
     * @param velocity 小球初始速度
     */
    setAgentDefaults(neighborDist: number, maxNeighbors: number, timeHorizon: number, timeHorizonObst: number, radius: number, maxSpeed: number, velocity: Vector2) {
        if (!this.defaultAgent) {
            this.defaultAgent = new Agent();
        }

        this.defaultAgent.maxNeighbors_ = maxNeighbors;
        this.defaultAgent.maxSpeed_ = maxSpeed;
        this.defaultAgent.neighborDist = neighborDist;
        this.defaultAgent.radius_ = radius;
        this.defaultAgent.timeHorizon = timeHorizon;
        this.defaultAgent.timeHorizonObst = timeHorizonObst;
        this.defaultAgent.velocity_ = velocity;
    }

    /**
     * 运行模拟（优化版）
     * @优化: 性能监控 + 批量处理
     */
    run(dt: number) {
        const frameStart = performance.now();
        this.stats.totalFrames++;
        
        // 1. 构建空间索引树
        const buildStart = performance.now();
        this.kdTree.buildAgentTree(this.getNumAgents());
        this.stats.buildTreeTime += performance.now() - buildStart;
        
        const agentNum = this.agentIdLst.length;
        
        // 优化: 缓存 agentIdLst 以减少重复访问
        const agentIds = this.agentIdLst;
        const agents = this.aid2agent;
        
        // 2. 计算邻居和新速度（合并循环以提高缓存命中率）
        const computeStart = performance.now();
        for (let i = 0; i < agentNum; i++) {
            const agent = agents[agentIds[i]];
            agent.computeNeighbors(this);
        }
        this.stats.computeNeighborsTime += performance.now() - computeStart;
        
        const velocityStart = performance.now();
        for (let i = 0; i < agentNum; i++) {
            agents[agentIds[i]].computeNewVelocity(dt);
        }
        this.stats.computeVelocityTime += performance.now() - velocityStart;
        
        // 3. 更新位置
        const updateStart = performance.now();
        for (let i = 0; i < agentNum; i++) {
            agents[agentIds[i]].update(dt);
        }
        this.stats.updateTime += performance.now() - updateStart;

        this.time += dt;
    }
    
    /**
     * 运行模拟（批量优化版）
     * @优化: 批量处理，减少循环开销
     */
    runOptimized(dt: number) {
        this.stats.totalFrames++;
        
        // 1. 构建树
        this.kdTree.buildAgentTree(this.getNumAgents());
        
        const agentNum = this.agentIdLst.length;
        const agentIds = this.agentIdLst;
        const agents = this.aid2agent;
        
        // 2. 批量处理所有步骤
        // 展开循环以提高性能（循环展开）
        let i = 0;
        const remainder = agentNum % 4;
        const limit = agentNum - remainder;
        
        // 主循环 - 每次处理 4 个代理
        for (; i < limit; i += 4) {
            const a0 = agents[agentIds[i]];
            const a1 = agents[agentIds[i + 1]];
            const a2 = agents[agentIds[i + 2]];
            const a3 = agents[agentIds[i + 3]];
            
            // 计算邻居
            a0.computeNeighbors(this);
            a1.computeNeighbors(this);
            a2.computeNeighbors(this);
            a3.computeNeighbors(this);
            
            // 计算新速度
            a0.computeNewVelocity(dt);
            a1.computeNewVelocity(dt);
            a2.computeNewVelocity(dt);
            a3.computeNewVelocity(dt);
            
            // 更新位置
            a0.update(dt);
            a1.update(dt);
            a2.update(dt);
            a3.update(dt);
        }
        
        // 处理剩余的代理
        for (; i < agentNum; i++) {
            const agent = agents[agentIds[i]];
            agent.computeNeighbors(this);
            agent.computeNewVelocity(dt);
            agent.update(dt);
        }

        this.time += dt;
    }

    addObstacle(vertices: Vector2[]) {
        if (vertices.length < 2) {
            return -1;
        }

        let obstacleNo = this.obstacles.length;

        for (let i = 0; i < vertices.length; ++i) {
            let obstacle = new Obstacle();
            obstacle.point = vertices[i];
            if (i != 0) {
                obstacle.previous = this.obstacles[this.obstacles.length - 1];
                obstacle.previous.next = obstacle;
            }
            if (i == vertices.length - 1) {
                obstacle.next = this.obstacles[obstacleNo];
                obstacle.next.previous = obstacle;
            }
            obstacle.direction = RVOMath.normalize(vertices[(i == vertices.length - 1 ? 0 : i + 1)].minus(vertices[i]));

            if (vertices.length == 2) {
                obstacle.convex = true;
            }
            else {
                obstacle.convex = (RVOMath.leftOf(vertices[(i == 0 ? vertices.length - 1 : i - 1)], vertices[i], vertices[(i == vertices.length - 1 ? 0 : i + 1)]) >= 0);
            }

            obstacle.id = this.obstacles.length;

            this.obstacles.push(obstacle);
        }

        return obstacleNo;
    }

    processObstacles() {
        this.kdTree.buildObstacleTree();
    };

    queryVisibility(point1: Vector2, point2: Vector2, radius: number) {
        return this.kdTree.queryVisibility(point1, point2, radius);
    };

    getObstacles() {
        return this.obstacles;
    }

    clear() {
        this.agentIdLst.length = 0;
        this.agentId = 0;
        this.aid2agent = Object.create(null);
        this.defaultAgent = null;
        this.kdTree = new KdTree();
        this.obstacles.length = 0;
        
        // 重置统计
        this.resetStats();
    }
    
    /**
     * 获取性能统计
     */
    getStats() {
        const totalTime = this.stats.buildTreeTime + 
                         this.stats.computeNeighborsTime + 
                         this.stats.computeVelocityTime + 
                         this.stats.updateTime;
        
        return {
            ...this.stats,
            totalTime,
            avgFrameTime: totalTime / Math.max(1, this.stats.totalFrames),
            buildTreePercent: (this.stats.buildTreeTime / totalTime * 100).toFixed(1),
            computeNeighborsPercent: (this.stats.computeNeighborsTime / totalTime * 100).toFixed(1),
            computeVelocityPercent: (this.stats.computeVelocityTime / totalTime * 100).toFixed(1),
            updatePercent: (this.stats.updateTime / totalTime * 100).toFixed(1)
        };
    }
    
    /**
     * 重置统计信息
     */
    resetStats(): void {
        this.stats = {
            buildTreeTime: 0,
            computeNeighborsTime: 0,
            computeVelocityTime: 0,
            updateTime: 0,
            totalFrames: 0
        };
    }
    
    /**
     * 打印性能统计
     */
    printStats(): void {
        const stats = this.getStats();
        
        console.log('━'.repeat(60));
        console.log('📊 Simulator 性能统计');
        console.log('━'.repeat(60));
        console.log(`总帧数: ${stats.totalFrames}`);
        console.log(`平均帧时间: ${stats.avgFrameTime.toFixed(2)}ms`);
        console.log(`构建树: ${stats.buildTreeTime.toFixed(2)}ms (${stats.buildTreePercent}%)`);
        console.log(`计算邻居: ${stats.computeNeighborsTime.toFixed(2)}ms (${stats.computeNeighborsPercent}%)`);
        console.log(`计算速度: ${stats.computeVelocityTime.toFixed(2)}ms (${stats.computeVelocityPercent}%)`);
        console.log(`更新位置: ${stats.updateTime.toFixed(2)}ms (${stats.updatePercent}%)`);
        console.log('━'.repeat(60));
    }
    
    /**
     * 批量添加代理（优化版）
     * @优化: 减少重复操作
     */
    addAgentsBatch(
        positions: Array<{ x: number; y: number }>,
        radius: number | null = null,
        maxSpeed: number | null = null,
        velocity: Vector2 | null = null,
        mass: number | null = null
    ): number[] {
        if (!this.defaultAgent) {
            throw new Error("no default agent");
        }
        
        const agentIds: number[] = [];
        const _radius = radius || this.defaultAgent.radius_;
        const _maxSpeed = maxSpeed || this.defaultAgent.maxSpeed_;
        const _velocity = velocity || this.defaultAgent.velocity_;
        const _mass = mass;
        
        for (const pos of positions) {
            const agent = new Agent();
            
            agent.position_.copy(pos);
            agent.maxNeighbors_ = this.defaultAgent.maxNeighbors_;
            agent.maxSpeed_ = _maxSpeed;
            agent.neighborDist = this.defaultAgent.neighborDist;
            agent.radius_ = _radius;
            agent.timeHorizon = this.defaultAgent.timeHorizon;
            agent.timeHorizonObst = this.defaultAgent.timeHorizonObst;
            agent.velocity_.copy(_velocity);
            agent.id = this.agentId++;
            
            if (_mass && _mass >= 0) {
                agent.mass = _mass;
            }
            
            this.aid2agent[agent.id] = agent;
            this.agentIdLst.push(agent.id);
            agentIds.push(agent.id);
        }
        
        return agentIds;
    }
    
    /**
     * 批量设置代理速度（优化版）
     * @优化: 批量操作
     */
    setAgentsPrefVelocityBatch(agentIds: number[], velocities: Array<{ x: number; y: number }>): void {
        const minLen = Math.min(agentIds.length, velocities.length);
        for (let i = 0; i < minLen; i++) {
            const agent = this.aid2agent[agentIds[i]];
            if (agent) {
                agent.prefVelocity_.copy(velocities[i]);
            }
        }
    }
    
    /**
     * 批量获取代理位置（优化版）
     * @优化: 批量查询，减少函数调用开销
     */
    getAgentPositionsBatch(agentIds: number[]): Vector2[] {
        const positions: Vector2[] = [];
        for (const aid of agentIds) {
            const agent = this.aid2agent[aid];
            if (agent) {
                positions.push(agent.position_.clone());
            }
        }
        return positions;
    }
}